#include "odegeomwrap.h"
#include "transform.h"
#include <osg/ComputeBoundsVisitor>
#include <osg/BoundingBox>
#include <ode/collision.h>
#include <osg/TriangleFunctor>
#include <osg/Geode>
#include <osg/ShapeDrawable>
#include <osg/Material>
#include <osg/PolygonMode>
#include <osg/PolygonOffset>
#include <osg/Geometry>
#include "datatypedefine.h"

namespace CoreBase
{
	ODEGeomWrap::ODEGeomWrap() : _geomID(NULL), _originalGeomID(NULL)
		, _triMeshDataID(NULL) , _meshVertices(NULL)
	{
		_geomID = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(_geomID, 1);
		dGeomTransformSetInfo(_geomID, 1);
		dGeomDisable(_geomID);
	}

	ODEGeomWrap::~ODEGeomWrap()
	{
		dGeomDestroy(_geomID);
		if (_triMeshDataID != NULL)
		{
			dGeomTriMeshDataDestroy(_triMeshDataID);
		}
		if (_meshVertices)
		{
			delete[] _meshVertices;
			_meshVertices = NULL;
			_numMeshVertices = 0;
		}
		_meshIndices.clear();
	}

	dGeomID ODEGeomWrap::getGeomID() const
	{
		return _geomID;
	}

	dGeomID ODEGeomWrap::getOriginalGeomID() const
	{
		return _originalGeomID;
	}

	QString ODEGeomWrap::getCollisionGeomType() const
	{
		int geomClass = dGeomGetClass(_geomID);
		dGeomID id = _geomID;
		while (geomClass == dGeomTransformClass && id != 0)
		{
			id = dGeomTransformGetGeom(id);
			if (id != 0)
			{
				geomClass = dGeomGetClass(id);
			}
		}
		switch (geomClass)
		{
		case dSphereClass:
			return COLLISION_TYPE_SPHERE;
		case dBoxClass:
			return COLLISION_TYPE_CUBE;
			//  case RAY:
			//     return &CollisionGeomType::RAY;
			//case CYLINDER:
			//     return &CollisionGeomType::CYLINDER;
			//  case CCYLINDER:
			//     return &CollisionGeomType::CCYLINDER;
		case dTriMeshClass:
			return COLLISION_TYPE_MESH;
		default:
			break;
		}
		return COLLISION_TYPE_NONE;
	}

	void ODEGeomWrap::getCollisionGeomDimensions(std::vector<float>& dimensions)
	{
		dimensions.clear();
		// The while loop is repeated here because we need both the type
		// and actual dGeomID of the geometry. GetCollisionGeomType only
		// return the type.
		int geomClass = dGeomGetClass(_geomID);
		dGeomID id = _geomID;
		while (geomClass == dGeomTransformClass && id != 0)
		{
			id = dGeomTransformGetGeom(id);
			if (id != 0)
			{
				geomClass = dGeomGetClass(id);
			}
		}
		switch (geomClass)
		{
		case dSphereClass:
			{
				dReal rad = dGeomSphereGetRadius(id);
				dimensions.push_back(float(rad));
				break;
			}
		case dBoxClass:
			{
				dVector3 sides;
				dGeomBoxGetLengths(id, sides);
				for (int i = 0; i < 3; i++)
				{
					dimensions.push_back(float(sides[i]));
				}
				break;
			}
		case dCylinderClass:
			{
				dReal radius, length;
				dGeomCylinderGetParams(id, &radius, &length);
				dimensions.push_back(float(radius));
				dimensions.push_back(float(length));
				break;
			}
		case dCCylinderClass:
			{
				dReal radius, length;
				dGeomCCylinderGetParams(id, &radius, &length);
				dimensions.push_back(float(radius));
				dimensions.push_back(float(length));
				break;
			}
		case dRayClass:
			{
				dVector3 start, dir;
				dReal length = dGeomRayGetLength(id);
				dGeomRayGet(id, start, dir);
				dimensions.push_back(length);
				for (int i = 0; i < 3; i++)
				{
					dimensions.push_back(float(start[i]));
				}
				for (int i = 0; i < 3; i++)
				{
					dimensions.push_back(float(dir[i]));
				}
				break;
			}
		case dTriMeshClass:
		default:
			break;
		}
	}

	void ODEGeomWrap::setCollisionDetection(bool enabled)
	{
		if (_geomID != 0)
		{
			int geomClass = dGeomGetClass(_geomID);
			dGeomID id = _geomID;
			while (geomClass == dGeomTransformClass && id != 0)
			{
				id = dGeomTransformGetGeom(id);
				if (id != 0)
				{
					geomClass = dGeomGetClass(id);
				}
			}
			// If we get here then a collision shape has been found.
			// Now we can enable/disable the GeomTransform at the top of the hierarchy.
			// Disabling the collision geometry itself will not prevent the transform
			// from colliding and still moving the collision hierarchy with it.
			if (enabled)
			{
				dGeomEnable(_geomID);
			}
			else
			{
				dGeomDisable(_geomID);
			}
		}
	}

	bool ODEGeomWrap::getCollisionDetection() const
	{
		if (_geomID != 0)
		{
			return dGeomIsEnabled(_geomID) == 1;
		}
		return false;
	}

	void ODEGeomWrap::setCollisionGeom(dGeomID geom)
	{
		dGeomTransformSetGeom(_geomID, geom);

	}

	void ODEGeomWrap::setCollisionSphere(float radius)
	{
		_originalGeomID = dCreateSphere(0, radius);
		dGeomDisable(_originalGeomID);
		dGeomTransformSetGeom(_geomID, dCreateSphere(0, radius));
		setCollisionDetection(true);
	}

	template< class T >
	class DrawableVisitor : public osg::NodeVisitor
	{
	public:
		osg::TriangleFunctor<T> mFunctor;
		DrawableVisitor(): osg::NodeVisitor(osg::NodeVisitor::TRAVERSE_ACTIVE_CHILDREN)
		{}
		/**
		* Applies this visitor to a geode.
		*
		* @param node the geode to visit
		*/
		virtual void apply(osg::Geode& node)
		{
			for (unsigned int i=0;i < node.getNumDrawables(); i++)
			{
				osg::Drawable* d = node.getDrawable(i);
				if (d->supports(mFunctor))
				{
					osg::NodePath nodePath = getNodePath();
					mFunctor.mMatrix = osg::computeLocalToWorld(nodePath);
					d->accept(mFunctor);
				}
			}
		}
	};

	class SphereFunctor
	{
	public:
		float mRadius;
		osg::Matrix mMatrix;
		SphereFunctor()
			: mRadius(0.0f)
		{}
		/**
		* Called once for each visited triangle.
		*
		* @param v1 the triangle's first vertex
		* @param v2 the triangle's second vertex
		* @param v3 the triangle's third vertex
		* @param treatVertexDataAsTemporary whether or not to treat the vertex data
		* as temporary
		*/
		void operator()(const osg::Vec3& v1,const osg::Vec3& v2,
			const osg::Vec3& v3, bool treatVertexDataAsTemporary)
		{
			osg::Vec3 tv1 = v1*mMatrix,
				tv2 = v2 * mMatrix,
				tv3 = v3 * mMatrix;
			tv1[2] = 0;
			if (tv1.length() > mRadius)
			{
				mRadius = tv1.length();
			}
			tv2[2] = 0;
			if (tv2.length() > mRadius)
			{
				mRadius = tv2.length();
			}
			tv3[2] = 0;
			if (tv2.length() > mRadius)
			{
				mRadius = tv3.length();
			}
		}
	};

	void ODEGeomWrap::setCollisionSphere(osg::Node* node)
	{
		if (node)
		{
			DrawableVisitor<SphereFunctor> sv;
			node->accept(sv);
			if (sv.mFunctor.mRadius > 0)
			{
				_originalGeomID = dCreateSphere(0, sv.mFunctor.mRadius);
				dGeomDisable(_originalGeomID);
				dGeomTransformSetGeom(_geomID, dCreateSphere(0, sv.mFunctor.mRadius));
			}
			else
			{
				//log
			}
		}
	}

	void ODEGeomWrap::setCollisionBox(float lx, float ly, float lz)
	{
		_originalGeomID = dCreateBox(0, lx, ly, lz);
		dGeomDisable(_originalGeomID);
		dGeomTransformSetGeom(_geomID, dCreateBox(0, lx, ly, lz));
	}

	void ODEGeomWrap::setCollisionBox(osg::Node* node)
	{
		if (node != 0)
		{
			osg::ComputeBoundsVisitor bbv;
			node->accept(bbv);
			float lx = bbv.getBoundingBox().xMax() - bbv.getBoundingBox().xMin();
			float ly = bbv.getBoundingBox().yMax() - bbv.getBoundingBox().yMin();
			float lz = bbv.getBoundingBox().zMax() - bbv.getBoundingBox().zMin();
			if (lx > 0.0f && ly > 0.0f && lz > 0.0f)
			{
				dGeomID subTransformID = dCreateGeomTransform(0);
				dGeomTransformSetCleanup(subTransformID, 1);
				_originalGeomID =  dCreateBox(0, lx, ly, lz);
				dGeomDisable(_originalGeomID);
				dGeomTransformSetGeom(subTransformID, dCreateBox(0,
					bbv.getBoundingBox().xMax() - bbv.getBoundingBox().xMin(),
					bbv.getBoundingBox().yMax() - bbv.getBoundingBox().yMin(),
					bbv.getBoundingBox().zMax() - bbv.getBoundingBox().zMin()));
				dGeomSetPosition(subTransformID,
					bbv.getBoundingBox().center()[0],
					bbv.getBoundingBox().center()[1],
					bbv.getBoundingBox().center()[2]);
				dGeomTransformSetGeom(_geomID, subTransformID);
			}
			else
			{
				//log
			}
		}
	}

	class CylinderFunctor
	{
	public:
		float mMinZ, mMaxZ, mRadius;
		osg::Matrix mMatrix;
		CylinderFunctor()
			: mMinZ(FLT_MAX), mMaxZ(-FLT_MAX), mRadius(0.0f)
		{}
		/**
		* Called once for each visited triangle.
		*
		* @param v1 the triangle's first vertex
		* @param v2 the triangle's second vertex
		* @param v3 the triangle's third vertex
		* @param treatVertexDataAsTemporary whether or not to treat the vertex data
		* as temporary
		*/
		void operator()(const osg::Vec3& v1,const osg::Vec3& v2,
			const osg::Vec3& v3, bool treatVertexDataAsTemporary)
		{
			osg::Vec3 tv1 = v1*mMatrix,
				tv2 = v2*mMatrix,
				tv3 = v3*mMatrix;
			if (tv1[2] < mMinZ) mMinZ = tv1[2];
			else if (tv1[2] > mMaxZ) mMaxZ = tv1[2];
			if (tv2[2] < mMinZ) mMinZ = tv2[2];
			else if (tv2[2] > mMaxZ) mMaxZ = tv2[2];
			if (tv3[2] < mMinZ) mMinZ = tv3[2];
			else if (tv3[2] > mMaxZ) mMaxZ = tv3[2];
			tv1[2] = 0;
			if (tv1.length() > mRadius) { mRadius = tv1.length(); }
			tv2[2] = 0;
			if (tv2.length() > mRadius) { mRadius = tv2.length(); }
			tv3[2] = 0;
			if (tv2.length() > mRadius) { mRadius = tv3.length(); }
		}
	};

	void ODEGeomWrap::setCollisionCylinder(float radius, float length)
	{
		_originalGeomID = dCreateCylinder(0, radius, length);
		dGeomDisable(_originalGeomID);
		dGeomTransformSetGeom(_geomID, dCreateCylinder(0, radius, length));
	}

	void ODEGeomWrap::setCollisionCylinder(osg::Node* node)
	{
		if (node)
		{
			DrawableVisitor<CylinderFunctor> cv;
			node->accept(cv);
			float radius = cv.mFunctor.mRadius;
			float length = cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ;
			if (radius > 0 && length > 0)
			{
				dGeomID subTransformID = dCreateGeomTransform(0);
				dGeomTransformSetCleanup(subTransformID, 1);
				_originalGeomID = dCreateCylinder(0, cv.mFunctor.mRadius, cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ);
				dGeomDisable(_originalGeomID);
				dGeomTransformSetGeom(subTransformID, dCreateCylinder(0, cv.mFunctor.mRadius, cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ));
				dGeomTransformSetGeom(_geomID, subTransformID);
			}
			else
			{
				//log
			}
		}
	}

	void ODEGeomWrap::setCollisionCappedCylinder(float radius, float length)
	{
		_originalGeomID = dCreateCCylinder(0, radius, length);
		dGeomDisable(_originalGeomID);
		dGeomTransformSetGeom(_geomID, dCreateCCylinder(0, radius, length));
	}

	void ODEGeomWrap::setCollisionCappedCylinder(osg::Node* node)
	{
		if (node)
		{
			DrawableVisitor<CylinderFunctor> cv;
			node->accept(cv);
			float radius = cv.mFunctor.mRadius;
			float length = cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ;
			if (radius > 0 && length > 0)
			{
				dGeomID subTransformID = dCreateGeomTransform(0);
				dGeomTransformSetCleanup(subTransformID, 1);
				_originalGeomID = dCreateCCylinder(0, cv.mFunctor.mRadius, cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ);
				dGeomDisable(_originalGeomID);
				dGeomTransformSetGeom(subTransformID, dCreateCCylinder(0, cv.mFunctor.mRadius, cv.mFunctor.mMaxZ - cv.mFunctor.mMinZ));
				dGeomTransformSetGeom(_geomID, subTransformID);
			}
			else
			{
				//log
			}
		}
	}

	void ODEGeomWrap::setCollisionRay(float length)
	{
		_originalGeomID = dCreateRay(0, length);
		dGeomDisable(_originalGeomID);
		dGeomTransformSetGeom(_geomID, dCreateRay(0, length));
	}

	struct StridedVertex
	{
		dVector3 Vertex;
	};

	struct StridedTriangle
	{
		int Indices[3];
	};

	class TriangleRecorder
	{
	public:
		std::vector<StridedVertex> mVertices;
		std::vector<StridedTriangle> mTriangles;
		osg::Matrix mMatrix;
		/**
		* Called once for each visited triangle.
		* @param v1 the triangle's first vertex
		* @param v2 the triangle's second vertex
		* @param v3 the triangle's third vertex
		* @param treatVertexDataAsTemporary whether or not to treat the vertex data
		* as temporary
		*/
		void operator()(const osg::Vec3& v1,const osg::Vec3& v2,
			const osg::Vec3& v3,bool treatVertexDataAsTemporary)
		{
			osg::Vec3 tv1 = v1*mMatrix,
				tv2 = v2*mMatrix,
				tv3 = v3*mMatrix;
			StridedVertex sv1, sv2, sv3;
			StridedTriangle t;
			t.Indices[0] = mVertices.size();
			t.Indices[1] = mVertices.size() + 1;
			t.Indices[2] = mVertices.size() + 2;
			mTriangles.push_back(t);
			sv1.Vertex[0] = tv1[0];
			sv1.Vertex[1] = tv1[1];
			sv1.Vertex[2] = tv1[2];
			sv2.Vertex[0] = tv2[0];
			sv2.Vertex[1] = tv2[1];
			sv2.Vertex[2] = tv2[2];
			sv3.Vertex[0] = tv3[0];
			sv3.Vertex[1] = tv3[1];
			sv3.Vertex[2] = tv3[2];
			mVertices.push_back(sv1);
			mVertices.push_back(sv2);
			mVertices.push_back(sv3);
		}
	};

	void ODEGeomWrap::setCollisionMesh(osg::Node* node)
	{
		if (node)
		{
			DrawableVisitor<TriangleRecorder> mv;
			node->accept(mv);
			if (_meshVertices != NULL)
			{
				delete[] _meshVertices;
				_meshVertices = NULL;
			}
			_numMeshVertices = mv.mFunctor.mVertices.size();
			if (_numMeshVertices > 0)
			{
				_meshVertices = new dVector3[_numMeshVertices];
			}
			_meshIndices.resize(mv.mFunctor.mTriangles.size()*3);
			if (!mv.mFunctor.mVertices.empty())
			{
				memcpy(&_meshVertices[0],
					&mv.mFunctor.mVertices[0],
					mv.mFunctor.mVertices.size()*sizeof(StridedVertex));
			}
			if (!mv.mFunctor.mTriangles.empty())
			{
				memcpy(&_meshIndices[0],
					&mv.mFunctor.mTriangles[0],
					mv.mFunctor.mTriangles.size()*sizeof(StridedTriangle));
			}
			if (_triMeshDataID == NULL)
			{
				_triMeshDataID = dGeomTriMeshDataCreate();
			}
			dGeomTriMeshDataBuildSimple(_triMeshDataID,
				(dReal*)_meshVertices, _numMeshVertices,
				_meshIndices.size() > 0 ? (dTriIndex*)&_meshIndices[0] : NULL, _meshIndices.size());
			dGeomTransformSetGeom(_geomID, dCreateTriMesh(0, _triMeshDataID, 0, 0, 0));
		}
	}

	void ODEGeomWrap::clearCollisionGeometry()
	{
		setCollisionDetection(false);
		dGeomTransformSetGeom(_geomID, 0);
	}

	void ODEGeomWrap::setCollisionCategoryBits(unsigned long bits)
	{
		dGeomSetCategoryBits(_geomID, bits);
	}

	unsigned long ODEGeomWrap::getCollisionCategoryBits() const
	{
		return dGeomGetCategoryBits(_geomID);
	}

	void ODEGeomWrap::setCollisionCollideBits(unsigned long bits)
	{
		dGeomSetCollideBits(_geomID, bits);
	}

	unsigned long ODEGeomWrap::getCollisionCollideBits() const
	{
		return dGeomGetCollideBits(_geomID);
	}

	osg::ref_ptr<osg::Geode> ODEGeomWrap::createRenderedCollisionGeometry(bool asWireFrame /* = false */) const
	{
		osg::ref_ptr<osg::Geode> geode = new osg::Geode();
		//geode->setName(Transformable::COLLISION_GEODE_ID);
		osg::TessellationHints* hints = new osg::TessellationHints;
		hints->setDetailRatio(0.5f);
		int geomClass = dGeomGetClass(getGeomID());
		dGeomID id = getGeomID();
		osg::Matrix absMatrix;
		while (geomClass == dGeomTransformClass)
		{
			id = dGeomTransformGetGeom(id);
			if (id == 0)
			{
				return NULL; // in case we haven't assigned a collision shape yet
			}
			geomClass = dGeomGetClass(id);
			const dReal* pos = dGeomGetPosition(id);
			const dReal* rot = dGeomGetRotation(id);
			osg::Matrix tempMatrix;
			tempMatrix(0,0) = rot[0];
			tempMatrix(0,1) = rot[1];
			tempMatrix(0,2) = rot[2];
			tempMatrix(1,0) = rot[4];
			tempMatrix(1,1) = rot[5];
			tempMatrix(1,2) = rot[6];
			tempMatrix(2,0) = rot[8];
			tempMatrix(2,1) = rot[9];
			tempMatrix(2,2) = rot[10];
			tempMatrix(3,0) = pos[0];
			tempMatrix(3,1) = pos[1];
			tempMatrix(3,2) = pos[2];
			absMatrix.postMult(tempMatrix);
		}
		switch (dGeomGetClass(id))
		{
		case dBoxClass:
			{
				dVector3 side;
				dGeomBoxGetLengths(getOriginalGeomID(), side);
				geode.get()->addDrawable(
					new osg::ShapeDrawable(
					new osg::Box(osg::Vec3(absMatrix(3,0), absMatrix(3,1), absMatrix(3,2)),
					side[0], side[1], side[2]), hints));
			}
			break;
		case dSphereClass:
			{
				dReal rad = dGeomSphereGetRadius(getOriginalGeomID());
				geode.get()->addDrawable(
					new osg::ShapeDrawable(
					new osg::Sphere(osg::Vec3(absMatrix(3,0), absMatrix(3,1), absMatrix(3,2)),
					rad), hints));
			}
			break;
		case dCylinderClass:
			{
				dReal radius, length;
				dGeomCylinderGetParams(getOriginalGeomID(), &radius, &length);
				geode.get()->addDrawable(
					new osg::ShapeDrawable(
					new osg::Cylinder(osg::Vec3(absMatrix(3,0), absMatrix(3,1), absMatrix(3,2)),
					radius, length), hints));
			}
			break;
		case dCCylinderClass:
			{
				dReal radius, length;
				dGeomCCylinderGetParams(getOriginalGeomID(), &radius, &length);
				geode.get()->addDrawable(
					new osg::ShapeDrawable(
					new osg::Capsule(osg::Vec3(absMatrix(3,0), absMatrix(3,1), absMatrix(3,2)),
					radius, length), hints));
			}
			break;
		case dTriMeshClass:
			{
				// just use the cached mesh data
				// pass the geometry to OSG
				osg::Geometry* geom = new osg::Geometry();
				// pack vertices
				{
					osg::Vec3Array* geomVertices = new osg::Vec3Array;
					for (int i = 0; i < _numMeshVertices; ++i)
					{
						geomVertices->push_back(osg::Vec3f(_meshVertices[i][0], _meshVertices[i][1], _meshVertices[i][2]));
					}
					geom->setVertexArray(geomVertices);
				}
				// pack indices
				{
					osg::DrawElementsUInt* geomIndices =
						new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, 0);
					for (size_t i = 0; i < _meshIndices.size(); ++i)
					{
						geomIndices->push_back(_meshIndices[i]);
					}
					geom->addPrimitiveSet(geomIndices);
				}
				geode.get()->addDrawable(geom);
			}
			break;
		case dPlaneClass:
			{
				//dVector4 result; //a*x+b*y+c*z = d
				//dGeomPlaneGetParams(id, result);
			}
		case dRayClass:
			{
				//dVector3 start, dir;
				//dReal length = dGeomRayGetLength(id);
				//dGeomRayGet(id, start, dir);
			}
		default:
			//log  dGeomGetClass(id);
			break;
		}
		osg::Material* mat = new osg::Material();
		mat->setDiffuse(osg::Material::FRONT_AND_BACK, osg::Vec4(1.f,0.f,1.f, 0.5f));
		mat->setAmbient(osg::Material::FRONT_AND_BACK, osg::Vec4(1.f,0.f,1.f, 1.f));
		mat->setEmission(osg::Material::FRONT_AND_BACK,osg::Vec4(0.f,0.f,0.f, 1.f));
		osg::PolygonOffset* polyoffset = new osg::PolygonOffset;
		polyoffset->setFactor(-1.0f);
		polyoffset->setUnits(-1.0f);
		osg::StateSet* ss = geode.get()->getOrCreateStateSet();
		ss->setAttributeAndModes(mat, osg::StateAttribute::OVERRIDE |
			osg::StateAttribute::PROTECTED | osg::StateAttribute::ON);
		ss->setMode(GL_BLEND,osg::StateAttribute::OVERRIDE |
			osg::StateAttribute::PROTECTED | osg::StateAttribute::ON);
		ss->setRenderingHint(osg::StateSet::TRANSPARENT_BIN);
		ss->setAttributeAndModes(polyoffset,osg::StateAttribute::OVERRIDE |
			osg::StateAttribute::PROTECTED | osg::StateAttribute::ON);
		if (asWireFrame)
		{
			osg::PolygonMode* pm = new osg::PolygonMode();
			pm->setMode(osg::PolygonMode::FRONT_AND_BACK,osg::PolygonMode::LINE);
			ss->setAttributeAndModes(pm,
				osg::StateAttribute::ON | osg::StateAttribute::OVERRIDE);
		}
		return geode;
	}

	void ODEGeomWrap::updateGeomTransform(const Transform& newTransform)
	{
		// If the supplied Transform is different than the current position
		// and rotation of the Geom, then set the Geom to match.
		if (getCollisionDetection() == false) { return; }
		Transform odeTransform;
		getGeomTransform(odeTransform);
		if (!newTransform.epsilonEquals(odeTransform))
		{
			osg::Matrix rotation;
			osg::Vec3 position;
			newTransform.getTranslation(position);
			newTransform.getRotation(rotation);
			// Set translation
			dGeomSetPosition(getGeomID(), position[0], position[1], position[2]);
			// Set rotation
			dMatrix3 dRot;
			dRot[0] = rotation(0,0);
			dRot[1] = rotation(1,0);
			dRot[2] = rotation(2,0);
			dRot[4] = rotation(0,1);
			dRot[5] = rotation(1,1);
			dRot[6] = rotation(2,1);
			dRot[8] = rotation(0,2);
			dRot[9] = rotation(1,2);
			dRot[10] = rotation(2,2);
			dGeomSetRotation(getGeomID(), dRot);
			int geomClass = dGeomGetClass(getGeomID());
			dGeomID id = getGeomID();
			while (geomClass == dGeomTransformClass)
			{
				id = dGeomTransformGetGeom(id);
				if (id == 0)
				{
					return; // In case we haven't assigned a collision shape yet
				}
				geomClass = dGeomGetClass(id);
			}
			if (id)
			{
				switch (dGeomGetClass(id))
				{
				case dBoxClass:
					{
						dVector3 originalSide;
						dGeomBoxGetLengths(getOriginalGeomID(), originalSide);
						dGeomBoxSetLengths(id, originalSide[0], originalSide[1], originalSide[2]);
					}
					break;
				case dSphereClass:
					{
						dReal originalRadius = dGeomSphereGetRadius(getOriginalGeomID());
						dGeomSphereSetRadius(id, originalRadius);
					}
					break;
				case dCCylinderClass:
					{
						dReal originalRadius, originalLength;
						dGeomCCylinderGetParams(getOriginalGeomID(), &originalRadius, &originalLength);
						dGeomCCylinderSetParams(id, originalRadius, originalLength);
					}
					break;
				case dRayClass:
					{
						dVector3 start, dir;
						dReal originalLength = dGeomRayGetLength(getOriginalGeomID());
						dGeomRayGet(getOriginalGeomID(), start, dir);
						// Ignore x/y scaling, use z to scale ray
						dGeomRaySetLength(id, originalLength);
					}
					break;
				case dTriMeshClass:
					{
						setCollisionMesh();
					}
					break;
				default:
					break;
				}
			}
		}
	}

	void ODEGeomWrap::getGeomTransform(Transform& xform) const
	{
		const dReal* position = dGeomGetPosition(getGeomID());
		const dReal* rotation = dGeomGetRotation(getGeomID());
		osg::Matrix newRotation;
		newRotation(0,0) = rotation[0];
		newRotation(1,0) = rotation[1];
		newRotation(2,0) = rotation[2];
		newRotation(0,1) = rotation[4];
		newRotation(1,1) = rotation[5];
		newRotation(2,1) = rotation[6];
		newRotation(0,2) = rotation[8];
		newRotation(1,2) = rotation[9];
		newRotation(2,2) = rotation[10];
		xform.setTranslation(position[0], position[1], position[2]);
		xform.setRotation(newRotation);
	}
}